
#include <cstdlib>
#include <iostream>
#include <sstream>

#include "typedefs.h"
#include "skeleton.h"
#include "skeletonLoader.h"
#include "motion.h"
#include "motionLoader.h"

using namespace std;

void writeASF(Skeleton *s, string filename, const vector<string> &toexclude);
void writeAMC(Motion *m, string filename, const vector<string> &toexclude);

// show the usage of program
void showUsage(string progName) {
   cout << "Usage: " << progName << " skeleton.asf motion.amc \n";
}

// main function
int main(int argc, char **argv) {
   
   if (argc < 2 or argc > 3) {
      showUsage(argv[0]);
      return (0);
   }
   
   string asfFile, amcFile;
   bool toExport = false;
   
   for (int i = 1; i < argc; i++) {
      string param = argv[i];
      if (param.substr(param.size() - 4, 4) == ".asf") {
         asfFile = param;
         if (i + 1 < argc) {
            i = i + 1;
            param = argv[i];
            if (param.substr(param.size() - 4, 4) == ".amc") {
               amcFile = param;
               toExport = true;
            } else {
               showUsage(argv[0]);
               return 0;
            }
         } else {
            showUsage(argv[0]);
            return 0;
         }
      } else {
         showUsage(argv[0]);
         return 0;
      }
   }
   
   if (not toExport) {
      showUsage(argv[0]);
      return 0;
   }
   
   string asfOutFile = asfFile + "_";
   string amcOutFile = amcFile + "_";
   
   cout<<asfOutFile<<" : "<<amcOutFile<<" : "<<endl;
   
   SkeletonLoader sl;
   Skeleton *s = sl.load(asfFile.c_str());
   assert(s!=0);
   
   MotionLoader ml;
   Motion *m = ml.load(amcFile.c_str());
   assert(m!=0);
   
   /// define the bones to exclude
   vector<string> toexclude;
   toexclude.push_back("ltoes");
   toexclude.push_back("rtoes");
   toexclude.push_back("lhand");
   toexclude.push_back("rhand");
   toexclude.push_back("lthumb");
   toexclude.push_back("rthumb");
   toexclude.push_back("lfingers");
   toexclude.push_back("rfingers");
   
   cout<<asfOutFile<<" : "<<amcOutFile<<" : "<<endl;
   
   writeASF(s, asfOutFile.c_str(), toexclude);
   writeAMC(m, amcOutFile.c_str(), toexclude);
   
   //~ cout<<m<<endl;
   
   return 0;
}

void writeASF(Skeleton *s, string filename, const vector<string> &toexclude) {
   
   Point3D rp = s->getRootPosition();
   string tr = s->getTrOrder();
   int id = 1;
   
   ofstream file(filename.c_str());
   file << "# ASF file generated by glutMOCAPviewer2.0" << endl;
   file << "# -----------------------------------------------" << endl;
   file << ":version 2.0" << endl;
   file << ":name glutMOCAPviewer2skel" << endl;
   file << ":units" << endl;
   file << "  mass 1.0" << endl;
   file << "  length 0.45" << endl;
   file << "  angle deg" << endl;
   file << ":documentation" << endl;
   file << "   .ast/.asf automatically generated by glutMOCAPviewer2.0" << endl;
   
   file << ":root" << endl;
   file << "   order T" << tr[0] << " T" << tr[1] << " T" << tr[2] << " R"
         << tr[3] << " R" << tr[4] << " R" << tr[5] << endl;
   file << "   axis XYZ" << endl;   
   file << "   position " << s->getRootPosition() << endl;  
   file << "   orientation 0 0 0" << endl;
   
   file << ":bonedata" << endl;
   map<string, Bone> b = s->getBones();
   for (map<string, Bone>::iterator i = b.begin(); i != b.end(); i++) {
      vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), i->first);
      if (result != toexclude.end()) continue;
      Bone bo = i->second;
      Vector3D dir = bo.getDirection();
      Vector3D rotAxis = bo.getRotationAxis();
      string dof = bo.getAxisOrder();
      file << "  begin" << endl;
      file << "     id " << id << endl; 
      file << "     name " << i->first << endl;
      file << "     direction " << dir[0] << " " << dir[1] << " " << dir[2] << endl;
      file << "     length " << bo.getLength() << endl;
      file << "     axis " << rotAxis[0] << " " << rotAxis[1] << " " << rotAxis[2] << " " << bo.getRotAxisOrder() << endl;
      file << "     dof";
      for (int i = 0; i < dof.size(); i++){
         file << " r" << dof[i];
      }
      file << endl;
      file << "  end" << endl;
      id++;
   }
   
   file << ":hierarchy" << endl;   
   Hierarchy h = s->getHierarchy();
   file << "  begin" << endl;
   //file << "    root lhipjoint rhipjoint lowerback" << endl;
   for (Hierarchy::iterator i = h.begin(); i != h.end(); i++) {
      vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), i->first);
      if (result == toexclude.end()) {
         file << "    " << i->first << " ";
         vector<string> v = i->second;
         for(vector<string>::iterator it = v.begin(); it != v.end(); it++){
            vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), *it);
            if (result == toexclude.end()) {
               file << *it << " ";
            }
         }
         file << endl;
      }
   }
    file << "  end" << endl;
    file.close();
   
   /// #####################################################
   /*
   Point3D rp = s->getRootPosition();
   string tr = s->getTrOrder();
   int id = 1;
   
   ofstream file(filename.c_str());
   file << "# ASF file generated by glutMOCAPviewer2.0" << endl;
   file << "# -----------------------------------------------" << endl;
   file << ":version 2.0" << endl;
   file << ":name glutMOCAPviewer2skel" << endl;
   file << ":units" << endl;
   file << "  mass 1.0" << endl;
   file << "  length 0.45" << endl;
   file << "  angle deg" << endl;
   file << ":documentation" << endl;
   file << "   .ast/.asf automatically generated by glutMOCAPviewer2.0" << endl;
   
   file << ":root" << endl;
   file << "   order T" << tr[0] << " T" << tr[1] << " T" << tr[2] << " R"
         << tr[3] << " R" << tr[4] << " R" << tr[5] << endl;
   file << "   axis XYZ" << endl;   
   file << "   position " << s->getRootPosition() << endl;  
   file << "   orientation 0 0 0" << endl;
   
   file << ":bonedata" << endl;
   map<string, Bone> b = s->getBones();
   for (map<string, Bone>::iterator i = b.begin(); i != b.end(); i++) {
      vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), i->first);
      if (result != toexclude.end()) continue;
      Bone bo = i->second;
      Vector3D dir = bo.getDirection();
      file << "  begin" << endl;
      file << "     id " << id << endl; 
      file << "     name " << i->first << endl;
      file << "     direction " << dir[0] << " " << dir[1] << dir[2] << endl;
      file << "     length " << bo.getLength() << endl; 
      file << "     axis " << bo.getAxisX() << " " << bo.getAxisY() << " " << bo.getAxisZ() << " XYZ" << endl;
      file << "  end" << endl;
      id++;
   }
   
   file << ":hierarchy" << endl;   
   Hierarchy h = s->getHierarchy();
   file << "  begin" << endl;
   //file << "    root lhipjoint rhipjoint lowerback" << endl;
   for (Hierarchy::iterator i = h.begin(); i != h.end(); i++) {
      vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), i->first);
      if (result == toexclude.end()) {
         file << "    " << i->first << " ";
         vector<string> v = i->second;
         for(vector<string>::iterator it = v.begin(); it != v.end(); it++){
            file << *it << " ";
         }
         file << endl;
      }
   }
    file << "  end" << endl;
    file.close();
    */
    cout << "Skeleton exported succefully!" << endl;
}

void writeAMC(Motion *m, string filename, const vector<string> &toexclude) {
   
   ofstream file(filename.c_str());
   file<<"#!OML:ASF exporter AMC"<<endl;
   file<<":FULLY-SPECIFIED"<<endl;
   file<<":DEGREES"<<endl;
   for (int i = 1; i < m->getMotionSize(); i++) {
      file << i << endl;
      PoseTransformations f = m->getFrame(i);
      PoseTransformations::iterator it;
      for (it = f.begin(); it != f.end(); it++) {
         vector<string>::const_iterator result = find(toexclude.begin(), toexclude.end(), it->first);
         if (result != toexclude.end()) continue;
         
         file << it->first;
         Transformations v = it->second;
         for (int j = 0; j < v.size(); j++) {
            file <<" "<< v[j];
         }
         file << endl;
      }      
   }
}
